#include "main_circle.hpp"
#include "Serial.hpp"
#include "uart.hpp"
Motor_Circle::Motor_Circle(std::string port, int baud) :  Serial(port, baud)
{
    open();
    mip= new Motor_Info_Press();
    for(int i=0;i<4;i++)
    {
        speed_set[i]=0;
        zhuan_set[i]=0;
        angle_set[i]=0;
        speed_cur[i]=0;
        zhuan_cur[i]=0;
        angle_cur[i]=0;
        linex=liney=wmga=0;
    }
    for(int i=0;i<4;++i)
    {
      bianma_id_rec_count[i]=0;
      motor_lun_id_rec_count[i]=0;
      zhaun_id_rec_count[i]=0;
    }
    send_sum_count=0;
    runThreadFunc();
    startTimer(7);
    //新开串口
    uart_zhuan = new Uart("ttyS7",1000000);
    uart_zhuan->open();
   // uart_zhuan->startTimer(8);
    uart_zhuan->runThreadFunc();
  
}
// export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
// export RMW_IMPLEMENTATION=rmw_fastrtps_dynamic_cpp

void Motor_Circle::topic_callback(std::vector<int32_t> &msg)
{

}
void  Motor_Circle::timer_callback()
{
   int i;
    swap_info();
    mip->run_set_angle();
    if(wmga!=0)liney=0;
    mip->Driver_cal_real(linex,liney,-wmga);
    mip->convert_line_real();
    swap_ctr();
  //  std::cout<<"speed ="<<mip->rpm_b[0]<<std::endl;
}
void Motor_Circle::recvMsg(std::vector<u_int8_t> &msg)
{
    //std::cout<<"hello"<<std::endl;
    int id;
    int16_t ag;
   if(rec_ok(msg)==0)
   {
    id=msg[1]*256+msg[2];
   // std::cout<<"id= "<<id<<std::endl;
    for(int i=0;i<4;i++)
        {
            if((bianma_id[i]+0x100)==id)
                {   
                    ag=msg[3]*256+msg[4];
                    angle_cur[i]=ag*bianma_negative[i];
                    bianma_id_rec_count[i]++;
                    if(bianma_id_rec_count[i]<0)bianma_id_rec_count[i]=0;
                    
                   // std::cout<<"angle"<<i<<"="<<angle_cur[i]<<std::endl;
                }
            if((motor_lun_id[i]+0x100)==id)
                {   
                    ag=(msg[3]<<8)|msg[4];
                    speed_cur[i]=ag*motor_lun_negative[i];
                    motor_lun_id_rec_count[i]++;
                    if(motor_lun_id_rec_count[i]<0)motor_lun_id_rec_count[i]=0;
                    // if(i==0)
                    // {
                        int32_t vva;
                       // std::cout<<"speed"<<i<<"="<<speed_cur[i]<<std::endl;
                        step_count[i]=msg[5]|(msg[6]<<8)|(msg[7]<<16)|(msg[8]<<24);
                        // for(int j=0;j<12;j++)
                        // std::cout<<" x"<<j<<"="<<toHex(msg[j]);
                        // std::cout<<std::endl;  
                        std::cout<<"step_"<<i<<"="<<step_count[i]<<std::endl;
                   // }
                  //  std::cout<<"angle"<<i<<"="<<angle_cur[i]<<std::endl;
                }    
            if((zhuan_id[i]+0x100)==id)
                {   
                    ag=msg[5]*256+msg[6];
                    zhuan_cur[i]=ag*zhuan_negative[i];
                    zhaun_id_rec_count[i]++;
                    if(zhaun_id_rec_count[i]<0)zhaun_id_rec_count[i]=0;
                  //  std::cout<<"angle"<<i<<"="<<angle_cur[i]<<std::endl;
                }        
        }
        msg.clear();
   }
}


void Motor_Circle::startTimer(int ms_s)
{
       interval = std::chrono::milliseconds(ms_s);   
     if (!running) {
            running = true;
            auto thread = std::thread([this]() {
                while (running) {
                //    callback(); // 执行回调
                   // if(sendbuf_vect.size()!=0)
                   // {
                   
                        if(circle_count==4)
                            {
                                circle_count=0;
                                Fucn_flag++;
                                if(Fucn_flag==3)Fucn_flag=0;
                                }
                        sendbuf_vect.clear();
                        if((Fucn_flag==1)&&(circle_count==1))send_sum_count++;
                        set_send_v(Fucn_flag,circle_count);
                        if(Fucn_flag==2)
                            uart_zhuan->write(sendbuf_vect);  //新开串口发送
                        
                        if(Fucn_flag!=2)
                            write(sendbuf_vect);
                       // std::cout << "send flag="<<Fucn_flag<<"cir=" <<circle_count<<std::endl;
                      //  send_count[circle_count]++;
                        // if(Reset_flag==1)  std::cout << "send clr"<< std::endl;
                        //     else std::cout << "send const"<< std::endl;
                       circle_count++; 
                       if(uart_zhuan->rec_flag==1)
                       {
                        recvMsg(uart_zhuan->msg_buf_);
                        uart_zhuan->rec_flag=0;
                       }
                      //  std::cout << "send const"<< std::endl;
                  //  }
                    std::this_thread::sleep_for(interval);
                }
            });
            thread.detach(); // 让定时器在后台运行，防止主线程阻塞
        }
}
int Motor_Circle::rec_ok(std::vector<u_int8_t> &msg)
{
    int flag=0;
    u_int8_t ch=0;
    if(msg[0]!=0xaa)return 1;
    if(msg.size()!=12)return 2;
    for(int i=0;i<11;i++)
        ch+=msg[i];
    if(ch==msg[11])    
        return flag;
      else return 3;  
}
void Motor_Circle::set_send_v(int n,int id)
{
    u_char ch;
    int16_t temp;
    switch(n)
    {
        case 0:  //读编码器角度
        sendbuf_vect.push_back(0xaa);
        ch=bianma_id[id]>>8;
        sendbuf_vect.push_back(ch);
        ch=bianma_id[id]&0xff;
        sendbuf_vect.push_back(ch);
        sendbuf_vect.push_back(0x01);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
         ch=0;
        for(int i=0;i<11;i++)ch+=sendbuf_vect[i];
        sendbuf_vect.push_back(ch);
        break; 
        case 1:  //轮子速度
        sendbuf_vect.push_back(0xaa);
        ch=motor_lun_id[id]>>8;
        sendbuf_vect.push_back(ch);
        ch=motor_lun_id[id]&0xff;
        sendbuf_vect.push_back(ch);
        
        //速度
        temp=speed_set[id]*motor_lun_negative[id];
        ch=temp>>8;
        sendbuf_vect.push_back(ch);
        ch=temp&0xff;
        sendbuf_vect.push_back(ch);
        //电流
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        //
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        ch=0;
        for(int i=0;i<11;i++)ch+=sendbuf_vect[i];
        //ch=0x0bd+(id&0xff);
        sendbuf_vect.push_back(ch);
        break;
        case 2:  //转向电机速度
        sendbuf_vect.push_back(0xaa);
        ch=zhuan_id[id]>>8;
        sendbuf_vect.push_back(ch);
        ch=zhuan_id[id]&0xff;
        sendbuf_vect.push_back(ch);
        sendbuf_vect.push_back(0x03);
        sendbuf_vect.push_back(0x01);
        //速度
        temp=zhuan_set[id]*zhuan_negative[id];
        ch=temp>>8;
        sendbuf_vect.push_back(ch);
        ch=temp&0xff;
        sendbuf_vect.push_back(ch);
        //电流
        sendbuf_vect.push_back(0x4e);
        sendbuf_vect.push_back(0x20);
        //
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        ch=0;
        for(int i=0;i<11;i++)ch+=sendbuf_vect[i];
        //ch=0x0bd+(id&0xff);
        sendbuf_vect.push_back(ch);
        break;
        //编码器置零
        case 100:
        sendbuf_vect.push_back(0xaa);
        ch=bianma_id[id]>>8;
        sendbuf_vect.push_back(ch);
        ch=bianma_id[id]&0xff;
        sendbuf_vect.push_back(ch);
        sendbuf_vect.push_back(0x02);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
         ch=0;
        for(int i=0;i<11;i++)ch+=sendbuf_vect[i];
        sendbuf_vect.push_back(ch);
        break;
    }
}

void Motor_Circle::swap_ctr(void)
{

    for(int i=0;i<4;i++)
    {
      //转向电机控制  
      zhuan_set[i]=mip->zhuan_speed_set[i];
      angle_set[i]=mip->angle_set[i];
      /////////////
      if(wmga==0)
      {
       // if(mip->lun_enable==1)
            speed_set[i]=mip->lun_speed_set[i];
        //    else speed_set[i]=mip->lun_speed_set[i]/2.0;
      }
      else
      {
        if(linex==0)
        {
        if(mip->lun_enable==1)
            speed_set[i]=mip->lun_speed_set[i];
            else speed_set[i]=0;
        }
        else  speed_set[i]=mip->lun_speed_set[i];
      }
       
    
    }
}
void Motor_Circle::swap_info(void)
{
    for(int i=0;i<4;i++)
    {
        //转向计算所需数据
       // mip->angle_set[i]=angle_set[i];
        mip->angle_cur[i]=angle_cur[i];
    /////////
        mip->lun_speed_cur[i]=speed_cur[i];
        mip->zhuan_speed_cur[i]=zhuan_cur[i];
     }
}
void Motor_Circle::set_motor_line(float x,float y,float z)
{
    linex=x;
    liney=y;
    wmga=z;
}